#ifndef MODEL_DIFFERENTIAL_KINEMATICS_H
#define MODEL_DIFFERENTIAL_KINEMATICS_H

#include "basic_data_structure.hpp"
#include "forward_kinematics.hpp"
#include "robot_model.hpp"

namespace ZORAL
{
    class DifferentialKinematics
    {
    private:
        std::shared_ptr<ForwardKinematics> fk_;
        std::shared_ptr<RobotModel> rm_;

    public:
        explicit DifferentialKinematics(std::shared_ptr<ForwardKinematics> fk)
        {
            fk_ = std::move(fk);
            rm_ = fk_->getRobotModel();
        }

        int calJacobian(const VectorXd &q, const bool &relate_to_base, Jacobian &jac) const;
    };
} // namespace ZORAL
#endif // MODEL_DIFFERENTIAL_KINEMATICS_H